#ifndef F_CPU
#define F_CPU 8000000UL
#endif

#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "lcd.h"
#include "fm.h"
#include "counter.h"
#include "wow.h"
#include "uart.h" //debug

#define TRUE 1
#define FALSE 0

// Function declarations
void fm_takeoff_task (void);
void fm_airborne_task (void);
void fm_landing_task (void);
void fm_bounce_task (void);
void fm_stop_task (void);
void check_wow (uint8_t, uint8_t, uint8_t);
void check_mov (uint8_t, uint16_t, uint8_t);


// Initialize flight manager variables
void fm_init(void) {

  Flight_Phase = TAKEOFF;
  Flight_Phase_Cng = 1;
  Got_Land_Mov = 0;

}

// Logic to define flight phase change from
// current flight phase and sensors readings
void fm_flight_phase(void) {

  switch(Flight_Phase) {

  case TAKEOFF:
    check_wow(FALSE,50,AIRBORNE);
    break;

  case AIRBORNE:
    check_wow(TRUE,100,LANDING);
    break;

  case LANDING:
    check_wow(FALSE,50,BOUNCE);
    if (Got_Land_Mov) check_mov(FALSE,1000,STOP);
    break;

  case BOUNCE:
    check_wow(TRUE,50,LANDING);
    break;

  }

}

// Execute appropriate flight phase task
void fm_phase_task(void) {

  switch(Flight_Phase) {
  
  case TAKEOFF:
    fm_takeoff_task();
    break;
  
  case AIRBORNE:
    fm_airborne_task();
    break;
  
  case LANDING:
    fm_landing_task();
    break;
  
  case BOUNCE:
    fm_bounce_task();
    break;
  
  case STOP:
    fm_stop_task();
    break;
  }
  
}


void fm_takeoff_task (void) {

  char msg[50];

  if (Flight_Phase_Cng) {
    // Init code
    counter_reset();
    Flight_Phase_Cng = 0;
    /* Debug */
    //sprintf(msg,"Phase: %d\n",Flight_Phase);
   //uart_transmit(msg);
    // Give enough time to flush serial buffer
    _delay_ms(10);
  }

  // Task code
  if (mov) {
    to_cnt += counter_get_cnts();
  }
  /* Debug */
  //sprintf(msg,"T.O. cnt: %u\n", to_cnt);
 // uart_transmit(msg);
  lcd_write(T,T,T,T,T,T,T,T);
 
}

void fm_airborne_task (void) {

  char msg[100];

  if (Flight_Phase_Cng) {
    // Init code
    clr_cnt_int();
    counter_reset();
    Flight_Phase_Cng = 0;
    /* Debug */
   // sprintf(msg,"Phase: %d\n",Flight_Phase);
    //uart_transmit(msg);
  }

  // Task code
  // In flight. Nothing for DAS to do.
  lcd_write('F','F','F','F','F','F','F','F');

}

void fm_landing_task (void) {

  char msg[100];

  if (Flight_Phase_Cng) {
    // Init code
    set_cnt_int();
    Flight_Phase_Cng = 0;
    /* Debug */
   //sprintf(msg,"Phase: %d\n",Flight_Phase);
   //uart_transmit(msg);
  }

  // Task code
  if (mov) {
    land_cnt += counter_get_cnts();
    if (Got_Land_Mov==0) Got_Land_Mov = 1;
  }
  /* Debug */
  //sprintf(msg,"LAND. cnt: %u\n",land_cnt);
  //uart_transmit(msg);

}

void fm_bounce_task (void) {

  char msg[100];

  if (Flight_Phase_Cng) {
    // Init code here
    clr_cnt_int();
    counter_reset();
    land_cnt += 50; //Por enquanto adicionando 1m
    Flight_Phase_Cng = 0;
    /* Debug */
    //sprintf(msg,"Phase: %d\n",Flight_Phase);
    //uart_transmit(msg);
  }

  // Task code

}

void fm_stop_task (void) {

  char msg[50];
  char to_str[10], ld_str[10];
  struct LCD_Chars to, ld; // Struct definido em lcd.h

  if (Flight_Phase_Cng) {
    // Init code
    clr_cnt_int();
    counter_reset();
    Flight_Phase_Cng = 0;
    /* Debug */
    //sprintf(msg,"Phase: %d\n",Flight_Phase);
    //uart_transmit(msg);
    _delay_ms(20);
  }

  // Task code
  to_dist = counter_dist_of_cnts(to_cnt);
  land_dist = counter_dist_of_cnts(land_cnt);

  // Escrever to_dist e land_dist no LCD.
  // 19/04/2011
  // Vou coletar os 8 chars necessarios e deixar
  // comentada a linha de chamada da funcao para o lcd.
  sprintf(to_str,"%4u",(unsigned int) to_dist);
  lcd_get_chars(to_str,&to); // Funcao definida em lcd.c 
  sprintf(ld_str,"%4u",(unsigned int) land_dist);
  lcd_get_chars(ld_str,&ld);
  lcd_write(to.iii,to.ii,to.i,to.o,ld.iii,ld.ii,ld.i,ld.o);
  
  /* Debug */
  //sprintf(msg,"T.O.: %u\n",(unsigned int) to_dist);
  //uart_transmit(msg);
  _delay_ms(20);
  //sprintf(msg,"LAND.: %u\n",(unsigned int) land_dist);
  //uart_transmit(msg);
}

void check_wow(uint8_t condition, uint8_t delay, uint8_t new_phase) {

  if (wow==condition) {
    _delay_ms(delay);
    if (wow==condition){
      Flight_Phase = new_phase;
      Flight_Phase_Cng = 1;
    }
  }
  
}

void check_mov(uint8_t condition, uint16_t delay, uint8_t new_phase) {

  if (mov==condition) {
    _delay_ms(delay);
    if (mov==condition){
      Flight_Phase = new_phase;
      Flight_Phase_Cng = 1;
    }
  }
  
}

